 '''导入一些基本的库'''
import cv2
import mediapipe as mp
import time
import numpy as np
from PIL import Image, ImageFont, ImageDraw
import random
import siot
# ------------------------------------------------
#   mediapipe的初始化
# 	这一步是必须的，因为要使用到以下定义的几个类
# ------------------------------------------------
mp_pose = mp.solutions.pose
mp_drawing = mp.solutions.drawing_utils
pose = mp_pose.Pose(static_image_mode=True)

SERVER = "192.168.31.233"
CLIENT_ID = ""
IOT_pubTopic  = '/secai'
IOT_UserName ='siot'
IOT_PassWord ='dfrobot'
siot.init(CLIENT_ID, SERVER, user=IOT_UserName, password=IOT_PassWord)
siot.connect()
siot.loop()

boo=1
waittime=0
def guan():
    mess="close"
    siot.publish(IOT_pubTopic, mess)

def kai():
    mess="open"
    siot.publish(IOT_pubTopic, mess)



def get_angle(v1, v2):
    angle = np.dot(v1, v2) / (np.sqrt(np.sum(v1 * v1)) * np.sqrt(np.sum(v2 * v2)))
    angle = np.arccos(angle) / 3.14 * 180

    cross = v2[0] * v1[1] - v2[1] * v1[0]
    if cross < 0:
        angle = - angle
    return angle

def OUTPING(a):
    if(a==1):
        kai()
        return 0
        #out open
    else:
        #out close
        guan()
        return 0

def get_pose(keypoints):
    str_pose = ""
    # 计算左臂与水平方向的夹角
    keypoints = np.array(keypoints)
    v1 = keypoints[12] - keypoints[11]
    v2 = keypoints[13] - keypoints[11]
    angle_left_arm = get_angle(v1, v2)
    #计算右臂与水平方向的夹角
    v1 = keypoints[11] - keypoints[12]
    v2 = keypoints[14] - keypoints[12]
    angle_right_arm = get_angle(v1, v2)
    #计算左肘的夹角
    v1 = keypoints[11] - keypoints[13]
    v2 = keypoints[15] - keypoints[13]
    angle_left_elow = get_angle(v1, v2)
    # 计算右肘的夹角
    v1 = keypoints[12] - keypoints[14]
    v2 = keypoints[16] - keypoints[14]
    angle_right_elow = get_angle(v1, v2)

    if angle_left_arm<0 and angle_right_arm>0 and abs(angle_right_elow)<120 and abs(angle_left_elow)<120:
        return 3
        #huanxing
   # print(angle_left_arm,angle_right_arm)
    if angle_left_arm<-165 and angle_left_arm>-195 and angle_right_arm<195 and angle_right_arm>165 :
        str_pose = 1#open
        #print(1)

    elif angle_left_arm<-75 and angle_left_arm>-105 and angle_right_arm<105 and angle_right_arm>75 :
        str_pose=2#close
        #print(2)
    else :
        str_pose=0
    return str_pose


def p(img):
    global boo
    global waittime
    h, w = img.shape[0], img.shape[1]               # 高和宽
    # 调整字体
    tl = round(0.005 * (img.shape[0] + img.shape[1]) / 2) + 1
    tf = max(tl-1, 1)
    # BRG-->RGB
    img_RGB = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
    # 将RGB图像输入模型，获取 关键点 预测结果
    results = pose.process(img_RGB)
    keypoints = ['' for i in range(33)]
    x = {}
    y = {}
    if results.pose_landmarks:
        mp_drawing.draw_landmarks(img, results.pose_landmarks, mp_pose.POSE_CONNECTIONS)
        for i in range(33):
            cx = int(results.pose_landmarks.landmark[i].x * w)
            cy = int(results.pose_landmarks.landmark[i].y * h)
            keypoints[i] = (cx,cy)                                 # 得到最终的33个关键点
            x[i]=cx
            y[i]=cy
    else:
        struction = "NO"
        img = cv2.putText(img, struction, (25, 100), cv2.FONT_HERSHEY_SIMPLEX, 1.25, (255, 255, 0),6)
        return img
    fps="YES"
    posep=get_pose(keypoints)
    if posep==3:
        waittime=100
    if waittime==0:
        return img
    if boo==1 :
        if posep==2:
            OUTPING(0)
            print("Close")
            fps="CLOSED"
            boo=0
    else:
        if posep==1:
            OUTPING(1)
            print("Open")
            fps="OPENED"
            boo=1
    #print(boo)
    waittime=max(0,waittime-1)
    colors = [[255 for _ in range(3)] for _ in range(33)]
    radius = [10 for _ in range(33)]
    for i in range(33):
        (cx,cy) = keypoints[i]
        if i in range(33):
            img = cv2.circle(img, (cx, cy), radius[i], colors[i], -1)
    cv2.putText(img, "FPS-{}".format(str(fps)), (12, 100), cv2.FONT_HERSHEY_SIMPLEX,tl/3, (255, 255, 0),thickness=tf)

camera=cv2.VideoCapture(0)
cv2.namedWindow('Cam',cv2.WINDOW_NORMAL)


while True:
    success,frame=camera.read()
    if success==0:
        print('off')
        continue
    img=p(frame)
    cv2.imshow('Cam',frame)
    if cv2.waitKey(125) != -1:
        break
    #cv2.destroyWindow('Cam')

camera.release()
cv2.destroyAllWindows()
